#include <stdlib.h>
#include <stdio.h>
#include "motor.h"
#include "i2c.h"




void setChannelASpeed(__u8 value){
  write_reg(MOTOR_CHIP,CHANNEL_A_SPEED, value);
}

void setChannelARun(){
  __u8 config_reg = read_reg(MOTOR_CHIP, CHANNELS_CONFIG);
  config_reg &= ~( CONFIG_MASK  << CHANNELA_DECAL) ;
  config_reg |= ( RUN << CHANNELA_DECAL) ;
  write_reg(MOTOR_CHIP, CHANNELS_CONFIG , config_reg);
}


void setChannelABrake(){
  __u8 config_reg = read_reg(MOTOR_CHIP, CHANNELS_CONFIG);
  config_reg &= ~( CONFIG_MASK  << CHANNELA_DECAL) ;
  config_reg |= ( BRAKE << CHANNELA_DECAL) ;
  write_reg(CHANNELS_CONFIG , config_reg);
}

void setChannelBSpeed(__u8 value){
  write_reg(MOTOR_CHIP, CHANNEL_B_SPEED, value);
}

void setChannelBRun(){
  __u8 config_reg = read_reg(MOTOR_CHIP, CHANNELS_CONFIG);
  config_reg &= ~( CONFIG_MASK  << CHANNELB_DECAL) ;
  config_reg |= ( RUN << CHANNELB_DECAL) ;
  write_reg(MOTOR_CHIP, CHANNELS_CONFIG , config_reg);
}

void setChannelBBrake(){
  __u8 config_reg = read_reg(MOTOR_CHIP, CHANNELS_CONFIG);
  config_reg &= ~( CONFIG_MASK  << CHANNELB_DECAL) ;
  config_reg |= ( BRAKE << CHANNELB_DECAL) ;
  write_reg(MOTOR_CHIP, CHANNELS_CONFIG , config_reg);
}

void setChannelADirection(int dir){
  __u8 config_reg = read_reg(MOTOR_CHIP, CHANNELS_CONFIG);
  if(dir){
     config_reg |=( DIR_MASK << CHANNELA_DECAL) ;
  }else{
     config_reg &= ~( DIR_MASK << CHANNELA_DECAL) ;
  }
  write_reg(MOTOR_CHIP, CHANNELS_CONFIG , config_reg);
}


void setChannelBDirection(int dir){
  __u8 config_reg = read_reg(MOTOR_CHIP, CHANNELS_CONFIG);
  if(dir){
     config_reg |=( DIR_MASK << CHANNELB_DECAL) ;
  }else{
     config_reg &= ~( DIR_MASK << CHANNELB_DECAL) ;
  }
  write_reg(MOTOR_CHIP, CHANNELS_CONFIG , config_reg);
}
